// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SuggestionDriveTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.UnitTests
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics.CodeAnalysis;
    using System.Linq;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// The Horizontal Depth Profile unit test class
    /// </summary>
    [TestClass]
    public class SuggestionDriveTests
    {
        /// <summary>
        /// This is a sample HDP taken from EV4A-007 facing a chair to the left
        /// and a desk to the right with several meters of open space in front.
        /// </summary>
        private static double[] realworldSample =
            new double[]
            {
                -2.00, -2.00, -2.00, -2.00, -2.00, 4.02, 3.93, 3.93, 4.02, 4.02,
                3.88, 3.88, 3.88, 3.93, 3.93, 3.92, 3.92, 4.01, 4.11, 4.11,
                4.11, 4.11, 4.16, -2.00, -2.00, 5.91, 5.91, 5.91, 5.93, 5.93,
                5.93, 5.93, 5.96, 5.96, 5.94, 5.95, 5.95, 5.92, 5.92, 5.95,
                5.95, 5.95, 5.95, 5.95, 6.00, 6.00, 6.00, 6.00, 6.00, 6.00,
                6.00, 6.00, 5.98, 5.98, 5.98, 5.99, 5.99, 5.94, 5.94, 5.99,
                5.99, 5.99, 5.99, 5.99, 5.99, 5.99, 5.99, 5.96, 5.86, 5.86,
                5.89, 5.89, 6.01, 6.01, 5.89, 5.89, 5.89, 5.89, 5.89, 5.89,
                5.89, 5.89, 5.89, 5.89, 5.89, 5.79, 5.79, 5.89, 5.89, 5.88,
                5.88, 5.88, 5.88, 6.02, 6.02, 5.88, 5.88, 5.88, 5.88, 5.88,
                5.88, 5.79, 5.79, 5.79, 5.79, 5.88, 5.88, 5.88, 5.88, 6.00,
                6.00, 6.00, 6.00, 5.90, 5.90, 6.00, 6.00, 5.90, 5.90, 5.90,
                5.90, 5.90, 5.90, 5.90, 5.90, 6.00, 6.00, 6.00, 6.00, 5.98,
                5.98, 5.98, 5.98, 5.98, 5.98, 5.98, 5.98, 5.98, 5.98, 5.98,
                5.98, 5.98, 5.98, 5.88, 5.88, 5.98, 5.98, 5.98, 5.98, 5.93,
                5.93, 6.03, 6.03, 5.93, 5.93, 5.93, 5.93, 5.93, -2.00, 4.54,
                -2.00, 4.14, 4.14, 4.14, 4.14, 4.19, 4.19, 4.19, 4.19, 4.16,
                4.16, 4.16, 4.16, 4.21, 4.21, 4.21, 4.21, 4.21, 4.16, 4.16,
                4.21, 4.21, 4.21, 4.21, 4.21, 4.21, 4.21, 4.21, 4.16, 4.16,
                4.14, 4.14, 4.14, 4.14, 4.14, 4.24, -2.00, 3.78, 3.78, 3.83,
                3.83, 3.78, 3.78, 3.74, 3.74, 3.74, 3.74, 3.78, 3.78, 3.78,
                3.78, 3.78, 3.75, 3.75, 3.75, 3.75, 3.75, 3.75, 3.71, 2.16,
                -2.00, 1.98, 1.98, 1.96, 1.96, 1.96, 1.96, 1.96, 1.96, 1.96,
                1.98, 1.98, 1.96, 1.96, 1.96, 1.96, 1.95, 1.95, 1.95, 1.92,
                1.92, 1.87, 1.87, 1.82, 1.82, 1.80, 1.80, 1.78, 1.78, 1.78,
                1.75, 1.75, 1.73, 1.73, 1.70, 1.70, 1.69, 1.69, 1.67, 1.67,
                1.67, 1.63, 1.63, 1.60, 1.60, 1.59, 1.59, 1.58, 1.58, 1.58,
                1.55, 1.55, 1.52, 1.52, 1.50, 1.50, 1.49, 1.49, 1.49, 1.48,
                1.48, 1.48, 1.48, 1.48, 1.48, 1.48, 1.48, 1.48, 1.48, 1.48,
                1.48, 1.48, 1.48, -2.00, 1.36, 1.36, 1.36, 1.34, 1.34, 1.34,
                1.34, 1.33, 1.33, 1.33, 1.33, 1.33, 1.31, 1.31, 1.28, 1.28,
                1.27, 1.27, 1.27, 1.26, 1.26, 1.25, 1.25, 1.23, 1.23, 1.23,
                1.22, 1.22, 1.22, 1.22, 1.21, 1.21, 1.19, 1.19, 1.19, 1.18,
                1.18, 1.17, 1.17, 1.17, 1.17, 1.17, 1.17, 1.17, 1.17, 1.17,
                1.11, 1.11, 1.11, 1.11, -2.00, 1.04, 1.04, 1.0, 3, 1.03,
                1.03, 1.03, 1.03, 1.03, 1.03, 1.04, 1.04, 1.04, 1.03, 1.03,
                1.02, 1.02, 1.02, 1.02, 1.0, 2, 1.02, 1.02, 1.02, 1.02,
                1.01, 1.01, 1.00, 1.00, 1.00, 0.99, 0.99, 0.98, 0.98, 0.97,
                0.97, 0.9, 7, 0.97, 0.97, 0.96, 0.96, 0.95, 0.95, 0.95,
                0.94, 0.94, 0.92, 0.92, 0.92, 0.92, 0.92, 0.91, 0.9, 1,
                0.91, 0.91, 0.90, 0.90, 0.90, 0.90, 0.90, 0.89, 0.89, 0.89,
                0.89, 0.89, 0.88, 0.88, 0.87, 0.8, 7, 0.86, 0.86, 0.86,
                0.85, 0.85, 0.85, 0.85, 0.84, 0.84, 0.84, 0.84, 0.84, 0.83,
                0.83, 0.82, 0.8, 2, 0.82, 0.82, 0.82, 0.80, 0.80, 0.80,
                0.80, 0.80, 0.80, 0.80, 0.79, 0.79, 0.79, 0.79, 0.79, 0.7,
                8, 0.78, 0.78, 0.78, 0.77, 0.77, 0.77, 0.76, 0.76, 0.76,
                0.76, 0.76, 0.76, 0.76, 0.75, 0.75, 0.7, 5, 0.75, 0.74,
                0.74, 0.74, 0.73, 0.73, 0.73, 0.73, 0.72, 0.72, 0.72, 0.72,
                0.72, 0.71, 0.71, 0.7, 1, 0.71, 0.71, 0.70, 0.70, 0.70,
                0.70, 0.69, 0.69, 0.69, 0.69, 0.69, 0.69, 0.69, 0.68, 0.68,
                0.6, 8, 0.68, 0.68, 0.67, 0.67, 0.67, 0.67, 0.67, 0.67,
                0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.6, 6, 0.66,
                0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.66, 0.67,
                0.67, 0.67, 0.67, 0.67, 0.6, 7, 0.67, 0.67, 0.67, 0.67,
                0.67, 0.67, 0.67, 0.67, 0.67, 0.67, 0.67, 0.67, 0.67, 0.66,
                0.66, 0.6, 5, 0.65, 0.65, 0.65, 0.65, 0.65, 0.65, 0.65,
                0.65, 0.65, 0.65, 0.65, 0.65, 0.64, 0.64, 0.64, 0.6, 4,
                0.64, 0.64, 0.64, 0.63, 0.63, 0.63, 0.63, 0.63, 0.63, 0.63,
                0.63, 0.62, 0.62, 0.62, 0.62, 0.6, 2, 0.62, 0.62, 0.62,
                0.62, 0.61, 0.61, 0.61, 0.61, 0.61, 0.61, 0.60, 0.60, 0.60,
                0.60, 0.60, 0.6, 0, 0.59, 0.59, 0.59, 0.59, 0.59, 0.59,
                0.59, 0.59, 0.59, 0.59, 0.59, 0.58, 0.58, 0.58, 0.58, 0.5,
                8, 0.58, 0.58, 0.58, 0.57, 0.57, 0.57, 0.57, 0.57, 0.57,
                0.57, 0.57, 0.57, -2.00, 0.56, -2.00, 0.56, -2.00, 0.56, -2.00,
                0.56, -2.00, 0.56, 0.56, -2.00, 0.56, -2.00, 0.56, -2.00, 0.56,
                -2.00, 0.55, -2.00, 0.55, -2.00, 0.55, -2.00
            };

        /// <summary>
        /// Test obstacle repelling factor by center proximity.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void ObstacleRepellingFactorByCenterProximityTest()
        {
            // Force is strongest in the center, exponentially falling off at the
            // edges. Effect is negative to the left, positive to the right.
            Assert.IsTrue(AllAreSame(
                    SuggestionDrive.ObstacleRepellingFactorByCenterProximity(10), 
                    new double[] { 0, -0.66, -1.16, -1.49, -1.66, 1.66, 1.5, 1.16, 0.66, 0 },
                    0.01));
        }

        /// <summary>
        /// Test obstacle repelling factor by distance.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void ObstacleRepellingFactorByDistanceTest()
        {
            // With a threshold of 0.5, force is 1.0 at that distance or nearer
            // and falls off exponentially from there.
            Assert.IsTrue(AllAreSame(
                    SuggestionDrive.ObstacleRepellingFactorByDistance(
                        Enumerable.Range(0, 100).Select(d => d / 10.0).ToArray(),
                        0.5), 
                    new double[]
                    {
                        1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 0.82, 0.69, 0.59, 0.51,
                        0.44, 0.39, 0.34, 0.30, 0.27, 0.25, 0.22, 0.20, 0.18, 0.17,
                        0.16, 0.14, 0.13, 0.12, 0.11, 0.11, 0.10, 0.09, 0.09, 0.08,
                        0.08, 0.07, 0.07, 0.06, 0.06, 0.06, 0.05, 0.05, 0.05, 0.05,
                        0.04, 0.04, 0.04, 0.04, 0.04, 0.04, 0.03, 0.03, 0.03, 0.03,
                        0.03, 0.03, 0.03, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.02,
                        0.02, 0.02, 0.02, 0.02, 0.02, 0.02, 0.01, 0.01, 0.01, 0.01,
                        0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01,
                        0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01,
                        0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.00, 0.00, 0.00, 0.00
                    },
                    0.01));
        }

        /// <summary>
        /// Test forward throttling while approaching a wall.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingFactorApproachingWall()
        {
            // Throttled to zero at 0.5m or nearer, growing to nearly 100% at 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.00, 0.00, 0.56, 0.75, 0.84, 0.92, 0.97, 0.98, 0.99 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                Wall,
                obstacle => SuggestionDrive.ForwardThrottlingFactor(obstacle, 0.5));
        }

        /// <summary>
        /// Test forward throttling while approaching a wide (0.5m) door.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingApproachingWideDoor()
        {
            // Throttled to ~2/3rds speed at the door, growing to nearly 100% at 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.68, 0.68, 0.85, 0.91, 0.94, 0.97, 0.98, 0.99, 0.99 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => Door(d, 0.5),
                obstacle => SuggestionDrive.ForwardThrottlingFactor(obstacle, 0.5));
        }

        /// <summary>
        /// Test forward throttling while approaching a narrow (0.25m) door.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingApproachingNarrowDoor()
        {
            // Throttled to ~1/3rds speed at the door, growing to nearly 100% at 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.37, 0.37, 0.72, 0.84, 0.90, 0.95, 0.98, 0.98, 0.99 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => Door(d, 0.25),
                obstacle => SuggestionDrive.ForwardThrottlingFactor(obstacle, 0.5));
        }

        /// <summary>
        /// Test forward throttling while approaching an obstacle to the left.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingApproachingLeftObstacle()
        {
            // Throttled to 85% while passing on the right, growing to nearly 100% af 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.84, 0.84, 0.92, 0.95, 0.97, 0.98, 0.99, 0.99, 0.99 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => LeftObstacle(d, 0.25),
                obstacle => SuggestionDrive.ForwardThrottlingFactor(obstacle, 0.5));
        }

        /// <summary>
        /// Test forward throttling while approaching an obstacle to the right.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingApproachingRightObstacle()
        {
            // Throttled to 85% while passing on the left, growing to nearly 100% af 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.85, 0.85, 0.93, 0.96, 0.97, 0.98, 0.99, 0.99, 0.99 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => RightObstacle(d, 0.25),
                obstacle => SuggestionDrive.ForwardThrottlingFactor(obstacle, 0.5));
        }

        /// <summary>
        /// Test forward throttling while approaching obstacles from real world HDP sample.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestForwardThrottlingApproachingRealWorldObstacle()
        {
            // Throttled to ~2/3rds speed.
            Assert.AreEqual<double>(
                0.63496,
                Math.Round(SuggestionDrive.ForwardThrottlingFactor(realworldSample, 0.5), 5));
        }

        /// <summary>
        /// Test heading adjustment while approaching a wall.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentFactorApproachingWall()
        {
            // No heading adjustment at all.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                Wall,
                obstacle => SuggestionDrive.HeadingAdjustment(obstacle, 0.05, 0.6));
        }

        /// <summary>
        /// Test heading adjustment while approaching a wide (0.5m) door.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentApproachingWideDoor()
        {
            // Very slight heading adjustments due to door being not *perfectly* centered.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { -0.06, -0.02, -0.01, -0.01, -0.01, 0.00, 0.00, 0.00, 0.00 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => Door(d, 0.5),
                obstacle => SuggestionDrive.HeadingAdjustment(obstacle, 0.05, 0.6));
        }

        /// <summary>
        /// Test heading adjustment while approaching a narrow (0.25m) door.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentApproachingNarrowDoor()
        {
            // Very slight heading adjustments due to door being not *perfectly* centered.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { -0.07, -0.03, -0.02, -0.01, -0.01, 0.00, 0.00, 0.00, 0.00 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => Door(d, 0.25),
                obstacle => SuggestionDrive.HeadingAdjustment(obstacle, 0.05, 0.6));
        }

        /// <summary>
        /// Test heading adjustment while approaching an obstacle to the left.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentApproachingLeftObstacle()
        {
            // Strong (> 1/2 radian) heading adjustment to the rigth at obstacle,
            // 1/3rd radian at threshold, falling off to zero at 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { -0.60, -0.33, -0.19, -0.12, -0.08, -0.04, -0.01, -0.01, 0.00 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => LeftObstacle(d, 0.25),
                obstacle => SuggestionDrive.HeadingAdjustment(obstacle, 0.05, 0.6));
        }

        /// <summary>
        /// Test heading adjustment while approaching an obstacle to the right.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentApproachingRightObstacle()
        {
            // Strong (> 1/2 radian) heading adjustment to the left at obstacle,
            // 1/3rd radian at threshold, falling off to zero at 9m.
            TestFunctionAgainstObstaclesAtVariousDistances(
                new double[] { 0.60, 0.31, 0.17, 0.11, 0.07, 0.04, 0.01, 0.01, 0.00 },
                new double[] { 0.00, 0.50, 1.00, 1.50, 2.00, 3.00, 5.00, 6.00, 9.00 },
                d => RightObstacle(d, 0.25),
                obstacle => SuggestionDrive.HeadingAdjustment(obstacle, 0.05, 0.6));
        }

        /// <summary>
        /// Test heading adjustment while approaching obstacles from real world HDP sample.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        public void TestHeadingAdjustmentApproachingRealWorldObstacle()
        {
            // Heading adjustment of 3+ radians (normally capped at 0.6).
            Assert.AreEqual<double>(
                3.16877,
                Math.Round(SuggestionDrive.HeadingAdjustment(realworldSample, 0.05, 10), 5));
        }

        /// <summary>
        /// Test that suggestion drive adjustment is working.
        /// </summary>
        /// <remarks>
        /// The core logic is already covered very thoroughly by other tests.
        /// This is mainly just basic test of the very edge API; that the unpacking
        /// from HDP and plumbing through are working.
        /// </remarks>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        [Ignore]
        public void AdjustDriveTest()
        {
            var hdp = new KinectHorizontalDepthProfile(new Pose2D(), new Vision.Cameras.DepthCameraIntrinsicParameters());

            // walls 1m away, but outside of 'corridor'
            for (var i = -1.0; i <= -0.8; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(1, i));
                hdp.AddHitPoint(new Point2D(1, -i));
            }

            var drive = new Motion.DriveDirection(
                heading: 0,
                forward: 1,
                distance: 0.5);

            var direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should have no effect
            Assert.AreEqual(direction.Forward, 1.0, 0.001);
            Assert.AreEqual(direction.Heading, 0, 0.001);

            // walls 1m away, just inside 'corridor'
            for (var i = -1.0; i <= -0.7; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(1, i));
                hdp.AddHitPoint(new Point2D(1, -i));
            }

            drive = new Motion.DriveDirection(
                heading: 0,
                forward: 1,
                distance: 0.5);

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should slow down just a bit approaching wall
            Assert.AreEqual(direction.Forward, 0.931, 0.001);

            // Heading should be virtually unchanged approaching straight on
            Assert.AreEqual(direction.Heading, 0, 0.001);

            // wall 5m away
            for (var i = -1.0; i <= 1.0; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(5, i));
            }

            drive = new Motion.DriveDirection(
                heading: 0,
                forward: 1,
                distance: 0.5);

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should slow down just a bit approaching wall
            Assert.AreEqual(direction.Forward, 0.921, 0.001);

            // Heading should be virtually unchanged approaching straight on
            Assert.AreEqual(direction.Heading, 0, 0.001);

            // add a small obstacle 1/2m away
            hdp.AddHitPoint(new Point2D(-0.5, 0.5));

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should slow down more with nearby obstacle
            Assert.AreEqual(direction.Forward, 0.804, 0.001);

            // Should veer away from obstacle slightly
            Assert.AreEqual(direction.Heading, -0.0208, 0.001);
        }

        /// <summary>
        /// Test that suggestion drive adjustment is working.
        /// </summary>
        /// <remarks>
        /// The core logic is already covered very thoroughly by other tests.
        /// This is mainly just basic test of the very edge API; that the unpacking
        /// from HDP and plumbing through are working.
        /// </remarks>
        [TestMethod]
        [TestCategory("Unit")]
        [TestCategory("Suggestion Drive")]
        [Priority(0)]
        [Ignore]
        public void AdjustDriveWithNoReadingsTest()
        {
            var hdp = new KinectHorizontalDepthProfile(new Pose2D(), new Vision.Cameras.DepthCameraIntrinsicParameters());

            // 'wall' of no-reading values
            var drive = new Motion.DriveDirection(
                heading: 0,
                forward: 1,
                distance: 0.5);

            var direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should have zero forward dampening
            Assert.AreEqual(direction.Forward, 1, 0.001);

            // Heading should be unchanged
            Assert.AreEqual(direction.Heading, 0, 0.001);

            // add valid, very distant wall to one side
            for (var i = -1.0; i <= 0.0; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(100, i));
            }

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should *still* have virtually zero forward dampening
            Assert.AreEqual(direction.Forward, 1, 0.001);

            // Heading should *still* be virtually unchanged
            Assert.AreEqual(direction.Heading, 0, 0.001);

            // move wall very close (1m) to see that it has effect
            for (var i = -1.0; i <= 0.0; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(1, i));
            }

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should *still* have virtually zero forward dampening
            Assert.AreEqual(direction.Forward, 0.745, 0.001);

            // Heading should *still* be virtually unchanged
            Assert.AreEqual(direction.Heading, -0.047, 0.001);

            // replace no-readings with distant readings
            for (var i = 0.0; i <= 0.9; i += 0.1)
            {
                hdp.AddHitPoint(new Point2D(100, i));
            }

            direction = SuggestionDrive.AdjustDrive(
                drive,
                hdp,
                distanceThreshold: 0.6,
                headingAdjustmentGain: 0.05,
                maximumHeadingAdjustment: 0.6,
                corridorRadiusInMeters: 1);

            // Should be same as before
            Assert.AreEqual(direction.Forward, 0.745, 0.001);

            // Should be same as before
            Assert.AreEqual(direction.Heading, -0.047, 0.001);
        }

        /// <summary>
        /// Test function against obstacles at various distances.
        /// </summary>
        /// <param name="expected">Expected values.</param>
        /// <param name="distances">Distances to test.</param>
        /// <param name="obstacleFn">Function to generate obstacles at a given distance.</param>
        /// <param name="testFn">Function being tested.</param>
        private static void TestFunctionAgainstObstaclesAtVariousDistances(
            IEnumerable<double> expected,
            IEnumerable<double> distances,
            Func<double, IEnumerable<double>> obstacleFn,
            Func<IEnumerable<double>, double> testFn)
        {
            // Generate obstacles at set of distances, run them through the given function
            // being tested and produce the differences between the results and the expected values.
            IEnumerable<double> differences = expected.Zip(
                distances,
                (expect, distance) =>
                {
                    IEnumerable<double> obstacle = obstacleFn(distance);
                    return Math.Round(testFn(obstacle) - expect, 2);
                });

            // If any are different from the expected, fail and report the differences.
            // Hint: To set up an initial test, give all zeros as expected.
            if (!differences.All(d => d == 0))
            {
                Assert.Fail(differences.Aggregate(string.Empty, (a, v) => a + string.Format("{0:0.00}, ", v)));
            }
        }

        /// <summary>
        /// Compare sets of double values within given epsilon.
        /// </summary>
        /// <param name="a">First set of double values.</param>
        /// <param name="b">Second set of double values.</param>
        /// <param name="epsilon">Maximum absolute difference considered equal.</param>
        /// <returns>Value indicating whether sets are identical.</returns>
        private static bool AllAreSame(IEnumerable<double> a, IEnumerable<double> b, double epsilon)
        {
            return a.Zip(b, (x, y) => Math.Abs(x - y) < epsilon).All(_ => _);
        }

        /// <summary>
        /// Build an HDP representing a wall at a given distance.
        /// </summary>
        /// <param name="distance">Distance to the wall.</param>
        /// <returns>HDP representing a wall.</returns>
        private static IEnumerable<double> Wall(double distance)
        {
            return Enumerable.Range(0, 100).Select(_ => distance);
        }

        /// <summary>
        /// Generate a range from -1 to +1.
        /// </summary>
        /// <remarks>Used to generate doors and left/right obstacles below.</remarks>
        /// <param name="count">Bin count.</param>
        /// <returns>Range from -1 to +1.</returns>
        private static IEnumerable<double> CenterProximity(int count)
        {
            return from b in Enumerable.Range(0, count)
                   select 2.0 / count * b - 1.0; // -1 .. +1
        }

        /// <summary>
        /// Build an HDP representing a door at a given distance.
        /// </summary>
        /// <param name="distance">Distance to the door.</param>
        /// <param name="width">Width of the door.</param>
        /// <returns>HDP representing a door.</returns>
        private static IEnumerable<double> Door(double distance, double width)
        {
            return CenterProximity(100).Select(c => Math.Abs(c) < width ? 10 : distance);
        }

        /// <summary>
        /// Build an HDP representing a left obstacle at a given distance.
        /// </summary>
        /// <param name="distance">Distance to the left obstacle.</param>
        /// <param name="width">Width of obstacle.</param>
        /// <returns>HDP representing a left obstacle.</returns>
        private static IEnumerable<double> LeftObstacle(double distance, double width)
        {
            return CenterProximity(100).Select(c => c < -(2 * width) ? distance : 10);
        }

        /// <summary>
        /// Build an HDP representing a right obstacle at a given distance.
        /// </summary>
        /// <param name="distance">Distance to the right obstacle.</param>
        /// <param name="width">Width of obstacle.</param>
        /// <returns>HDP representing a right obstacle.</returns>
        private static IEnumerable<double> RightObstacle(double distance, double width)
        {
            return CenterProximity(100).Select(c => c > 1 - 2 * width ? distance : 10);
        }
   }
}
